//
// Created by cch on 22-5-2.
//

#ifndef IMU_GPS_LOCALIZATION_LOCALIZATIONWRAPPER_H
#define IMU_GPS_LOCALIZATION_LOCALIZATIONWRAPPER_H

#include "base_type.h"
#include "imu_gps_fusion.h"
#include <fstream>
#include <iostream>
#include <nav_msgs/Path.h>
#include <ros/ros.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/NavSatFix.h>

class LocalizationWrapper {
public:
  LocalizationWrapper(ros::NodeHandle &nh) {
    double acc_noise, gyro_noise, acc_bias_noise, gyro_bias_noise;
    nh.param("acc_noise", acc_noise, 1e-2);
    nh.param("gyro_noise", gyro_noise, 1e-4);
    nh.param("acc_bias_noise", acc_bias_noise, 1e-6);
    nh.param("gyro_bias_noise", gyro_bias_noise, 1e-8);

    double x, y, z;
    nh.param("I_P_GPS", x, 0.0);
    nh.param("I_P_GPS", y, 0.0);
    nh.param("I_P_GPS", z, 0.0);

    const Eigen::Vector3d I_P_GPS(x, y, z);
    imu_gps_fusion_ptr_ = std::make_unique<IMUGPS::Fusion>(
        acc_noise, gyro_noise, acc_bias_noise, gyro_bias_noise, I_P_GPS);
    imu_sub_ =
        nh.subscribe("imu_data", 10, &LocalizationWrapper::imuCallBack, this);
    gps_position_sub_ = nh.subscribe(
        "gps_data", 10, &LocalizationWrapper::GpsPositionCallBack, this);
    state_pub_ = nh.advertise<nav_msgs::Path>("fused_data", 10);
  }
  ~LocalizationWrapper() {
    file_gps_.close();
    file_state_.close();
  }

  void imuCallBack(const sensor_msgs::ImuConstPtr &imu_msg_ptr);
  void GpsPositionCallBack(const sensor_msgs::NavSatFixConstPtr &gps_msg_ptr);

private:
  void LogState(const IMUGPS::State &state);
  void LogGps(const IMUGPS::GpsDataPtr &gps_data_ptr);
  void ConvertStateToRosTopic(const IMUGPS::State *state);

  std::ofstream file_state_;
  std::ofstream file_gps_;
  // 话题订阅者
  ros::Subscriber imu_sub_;
  ros::Subscriber gps_position_sub_;
  // 状态发布者
  ros::Publisher state_pub_;
  nav_msgs::Path ros_path_;
  std::unique_ptr<IMUGPS::Fusion> imu_gps_fusion_ptr_;
};

#endif // IMU_GPS_LOCALIZATION_LOCALIZATIONWRAPPER_H
